#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <foxglove_msgs/msg/compressed_image.hpp>
#include <builtin_interfaces/msg/time.hpp>
#include <opencv2/opencv.hpp>


class ImageRepublisher : public rclcpp::Node
{
public:
    ImageRepublisher() : Node("image_republisher")
    {
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/aurora/rgb/image_raw", 3,
            std::bind(&ImageRepublisher::listener_callback_image, this, std::placeholders::_1));

        publisher_ = this->create_publisher<foxglove_msgs::msg::CompressedImage>("/image_compressed", 10);
    }

private:
    void listener_callback_image(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        // 将 ROS 图像消息转换为 OpenCV Mat
        cv::Mat cv_image(
            msg->height, 
            msg->width, 
            CV_8UC3,  // BGR8 格式
            const_cast<unsigned char*>(msg->data.data())
        );

        // 压缩为 JPEG 格式
        std::vector<uchar> jpeg_buffer;
        int encode_quality = 95;  // 压缩质量 (0-100)
        cv::imencode(".jpg", cv_image, jpeg_buffer, {cv::IMWRITE_JPEG_QUALITY, encode_quality});
        
        // 创建 CompressedImage 消息
        auto image_republisher = foxglove_msgs::msg::CompressedImage();
        image_republisher.timestamp.sec = msg->header.stamp.sec;
        image_republisher.timestamp.nanosec = msg->header.stamp.nanosec;  // 设置当前时间的纳秒部分
        image_republisher.frame_id = msg->header.frame_id;
        image_republisher.format = "jpeg";
        image_republisher.data = jpeg_buffer;

        // 发布压缩后的图像消息
        publisher_->publish(image_republisher);
    }



    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
    rclcpp::Publisher<foxglove_msgs::msg::CompressedImage>::SharedPtr publisher_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImageRepublisher>());
    rclcpp::shutdown();
    return 0;
}
